{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "intro_md"
      },
      "source": [
        "# SmartProjectionFactor"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "desc_md"
      },
      "source": [
        "`SmartProjectionFactor<CAMERA>` is a \"smart\" factor designed for Structure from Motion (SfM) or visual SLAM problems where **both camera poses and calibration are being optimized**.\n",
        "It implicitly represents a 3D point landmark that has been observed by multiple cameras.\n",
        "\n",
        "Key characteristics:\n",
        "- **Implicit Point:** The 3D point is not an explicit variable in the factor graph. Instead, the factor internally triangulates the point based on the current camera estimates whenever needed (e.g., during linearization or error calculation).\n",
        "- **Marginalization:** When linearized (e.g., to a Hessian factor), it effectively marginalizes out the 3D point, creating a dense factor connecting only the cameras that observed the point.\n",
        "- **`CAMERA` Template:** This template parameter must represent a camera model that includes *both* pose and calibration (e.g., `PinholeCameraCal3_S2`, `PinholeCameraBundler`).\n",
        "- **`Values` Requirement:** When using this factor, the `Values` object passed to methods like `error` or `linearize` must contain `CAMERA` objects (not separate `Pose3` and `Calib` objects) associated with the factor's keys.\n",
        "- **Configuration:** Its behavior (linearization method, handling of degenerate triangulations) is controlled by `SmartProjectionParams`.\n",
        "\n",
        "**Use Case:** Suitable for Bundle Adjustment or SfM problems where calibration parameters are unknown or need refinement along with camera poses.\n",
        "**Alternative:** If calibration is known and fixed, use `SmartProjectionPoseFactor` for better efficiency.\n",
        "\n",
        "If you are using the factor, please cite:\n",
        "> **L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert**, \"Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors\", Int. Conf. on Robotics and Automation (ICRA), 2014."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "colab_badge_md"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/SmartProjectionFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "id": "pip_code",
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "try:\n",
        "    import google.colab\n",
        "    %pip install --quiet gtsam-develop\n",
        "except ImportError:\n",
        "    pass  # Not running on Colab, do nothing"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {
        "id": "imports_code"
      },
      "outputs": [],
      "source": [
        "import gtsam\n",
        "import numpy as np\n",
        "from gtsam import (\n",
        "    Values,\n",
        "    Point2,\n",
        "    Point3,\n",
        "    Pose3,\n",
        "    Rot3,\n",
        "    NonlinearFactorGraph,\n",
        "    SmartProjectionParams,\n",
        "    SmartProjectionFactorPinholeCameraCal3_S2,\n",
        "    PinholeCameraCal3_S2,\n",
        "    Cal3_S2,\n",
        ")\n",
        "from gtsam.symbol_shorthand import C"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "create_header_md"
      },
      "source": [
        "## Creating and Adding Measurements"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "create_desc_md"
      },
      "source": [
        "1. Create the factor with a noise model and parameters.\n",
        "2. Add measurements (2D points) and the corresponding camera keys one by one or in batches."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "metadata": {
        "id": "create_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Smart factor involves 3 measurements.\n",
            "SmartFactor: SmartProjectionFactor\n",
            "linearizationMode: 0\n",
            "triangulationParameters:\n",
            "rankTolerance = 1\n",
            "enableEPI = 0\n",
            "landmarkDistanceThreshold = -1\n",
            "dynamicOutlierRejectionThreshold = -1\n",
            "useLOST = 0\n",
            "noise model\n",
            "\n",
            "result:\n",
            "no point, status = 1\n",
            "\n",
            "SmartFactorBase, z = \n",
            "measurement 0, px = \n",
            "150\n",
            "505\n",
            "noise model = unit (2) \n",
            "measurement 1, px = \n",
            "470\n",
            "495\n",
            "noise model = unit (2) \n",
            "measurement 2, px = \n",
            "480\n",
            "150\n",
            "noise model = unit (2) \n",
            "  keys = { c0 c1 c2 }\n"
          ]
        }
      ],
      "source": [
        "smart_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n",
        "smart_params = SmartProjectionParams() # Use default params (HESSIAN, IGNORE_DEGENERACY)\n",
        "\n",
        "# Factor type includes the Camera type, e.g., SmartProjectionFactorPinholeCameraCal3_S2\n",
        "smart_factor = SmartProjectionFactorPinholeCameraCal3_S2(smart_noise, smart_params)\n",
        "\n",
        "# Add measurements and keys\n",
        "smart_factor.add(Point2(150, 505), C(0))\n",
        "smart_factor.add(Point2(470, 495), C(1))\n",
        "smart_factor.add(Point2(480, 150), C(2))\n",
        "\n",
        "print(f\"Smart factor involves {smart_factor.size()} measurements.\")\n",
        "smart_factor.print(\"SmartFactor: \")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "eval_header_md"
      },
      "source": [
        "## Evaluating the Error"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "eval_desc_md"
      },
      "source": [
        "The `.error(values)` method implicitly triangulates the point based on the `CAMERA` objects in `values` and computes the sum of squared reprojection errors."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "metadata": {
        "id": "eval_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Triangulated point result:\n",
            "Status.BEHIND_CAMERA\n",
            "\n",
            "Triangulation failed, error calculation depends on degeneracyMode.\n"
          ]
        }
      ],
      "source": [
        "# Create Values containing CAMERA objects\n",
        "values = Values()\n",
        "K = Cal3_S2(500, 500, 0, 320, 240)\n",
        "pose0 = Pose3(Rot3.Ypr(0.1, -0.1, 0.2), Point3(-1, 0, 0.5))\n",
        "pose1 = Pose3(Rot3.Ypr(0.0,  0.1, 0.1), Point3( 1, 0, 0.5))\n",
        "pose2 = Pose3(Rot3.Ypr(-0.1, 0.0, -0.2), Point3( 0, 1, 0.5))\n",
        "\n",
        "values.insert(C(0), PinholeCameraCal3_S2(pose0, K))\n",
        "values.insert(C(1), PinholeCameraCal3_S2(pose1, K))\n",
        "values.insert(C(2), PinholeCameraCal3_S2(pose2, K))\n",
        "\n",
        "# Triangulate first to see the implicit point\n",
        "point_result = smart_factor.point(values)\n",
        "print(f\"Triangulated point result:\\n{point_result.status}\")\n",
        "\n",
        "if point_result.valid():\n",
        "   # Calculate error\n",
        "   total_error = smart_factor.error(values)\n",
        "   print(f\"\\nTotal reprojection error (0.5 * sum(err^2/sigma^2)): {total_error:.4f}\")\n",
        "else:\n",
        "   print(\"\\nTriangulation failed, error calculation depends on degeneracyMode.\")"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "py312",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.6"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}
